const int ledPin = 13;

unsigned long startTime;

void setup() {  
  pinMode(ledPin, OUTPUT);
  pinMode(6, INPUT);
  motorSetup();
  sensSetup();
  Serial.begin(115200);  
  Serial.println("WALL-E V2 says hello!");   
  blink(3);
  startTime=millis();
}

void loop() {  
  //1= right; 2= MR; 3 =ML; 4=Left
  
  testEnc(); 
  
  UpdateSpeed();

  if(sens()==0){
    forward();
  }
  else{
    stopp();
  }

  //Right sensors
  while(sens()==1){
    right();
    UpdateSpeed();
  }

  if(sens()==2){
    shortback(250);
    left();
    delay(150);
    stopp();
    while(sens()==2){
      left();
      UpdateSpeed();
    }
  } 

  //Left sensors
  while(sens()==4){
    left();
    UpdateSpeed();
  } 
  
  if(sens()==3){
    shortback(250);
    right();
    delay(150);
    stopp();
    while(sens()==3){
      right();
      UpdateSpeed();
    }
  }

  delay(10); 
}

void set(byte pin, byte value){
  digitalWrite(pin, value); 
}

void blink(int a) {
  for(int i=0;i<a;i++){ 
    set(ledPin, HIGH);   // set the LED on
    delay(70);              // wait for a second
    set(ledPin, LOW);    // set the LED off
    delay(230);              // wait for a second 
  }
}


unsigned long lastLog;

void logMsg(char msg) {
/*  int diff=millis()-lastLog;
  if(diff>=200){
    Serial.println(msg);  
    delay(1);
    lastLog = millis();
  }*/
}


